#include "synth.h"
#include <algorithm>
#include <cmath>
#include <sstream>
#include <stdexcept>

using cd = std::complex<double>;
//计算传递函数𝐻(𝑧)在某个频率点ω的复数响应值
std::complex<double> tf_eval(const TF& H, double w) {
  cd z = std::exp(cd(0.0, w));     // e^{jw}
  cd zinv = cd(1.0, 0.0) / z;      // z^{-1}
  cd N(0.0, 0.0), D(0.0, 0.0);
  cd zpow(1.0, 0.0);

  for (double b : H.num) {
    N += b * zpow;
    zpow *= zinv;
  }
  zpow = cd(1.0, 0.0);
  for (double a : H.den) {
    D += a * zpow;
    zpow *= zinv;
  }
  return N / D;
}

//向量最大值
double max_value(const std::vector<double>& lambdas) {
  if (lambdas.empty()) throw std::invalid_argument("lambdas is empty");
  return *std::max_element(lambdas.begin(), lambdas.end());
}

// 计算复矩阵的最大奇异值（2范数）
inline double sigma_max(const Eigen::MatrixXcd& H) {
  // H^* H 是 Hermitian，最大特征值的平方根就是最大奇异值
  Eigen::MatrixXcd HhH = H.adjoint() * H;
  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXcd> es(HhH);
  return std::sqrt( std::max(0.0, es.eigenvalues().real().maxCoeff()) );
}

// MIMO H∞（状态空间版）：在单位圆上扫频，取最大奇异值
// worN: 频点个数；radius<1 可避免极点贴在单位圆导致的数值不稳
double hinf_norm(const StateSpace& G, int worN, double radius ) {
  const int nx = G.A.rows();
  const int ny = G.C.rows();
  const int nu = G.B.cols();

  // 无状态时直接返回常数矩阵 D 的 2-范数
  if (nx == 0) {
    Eigen::MatrixXcd D = G.D.cast<std::complex<double>>();
    return sigma_max(D);
  }

  using cmat = Eigen::MatrixXcd;
  cmat A = G.A.cast<std::complex<double>>();
  cmat B = G.B.cast<std::complex<double>>();
  cmat C = G.C.cast<std::complex<double>>();
  cmat D = G.D.cast<std::complex<double>>();

  double sup = 0.0;
  for (int k = 0; k < worN; ++k) {
    const double w = 2.0 * M_PI * (double)k / worN;
    std::complex<double> z = std::polar(radius, w);         // z = r·e^{jw}
    cmat ZwI = z * cmat::Identity(nx, nx) - A;               // (zI - A)

    // 求 H(z) = C (zI - A)^{-1} B + D
    // 用解线性方程代替显式求逆，数值更稳：
    cmat X = ZwI.fullPivLu().solve(B);                       // (zI-A)^{-1} B
    cmat H = C * X + D;                                      // ny x nu

    sup = std::max(sup, sigma_max(H));
  }
  return sup;
}

//求 实对称矩阵特征值
std::vector<double> laplacian_eigs(const std::vector<std::vector<double>>& L) {
  const int n = (int)L.size();
  Eigen::MatrixXd M(n,n);
  for(int i=0;i<n;i++) for(int j=0;j<n;j++) M(i,j)=L[i][j];
  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(M); // 对称阵
  std::vector<double> eigs(n);
  for (int i=0;i<n;i++) eigs[i]=es.eigenvalues()(i);
  return eigs; // 已从小到大
}


// 步骤 2：由 λ, τ, γ 计算论文参数
PaperParams paper_params(double lambda, double tau, double gamma) {
  PaperParams P{};

  P.tau = tau;
  P.gamma = gamma;

  // p1 和 p1_tilde
  P.p1 = (tau * tau * lambda * lambda + tau * lambda * sqrt(4.0 + tau * tau * lambda * lambda)) / 2.0;
  P.p1_tilde = 1.0 + P.p1;

  // p_gamma
  P.p_gamma = gamma * pow(P.p1_tilde, -2.0) + (1.0 / P.p1_tilde) * (lambda * lambda) - gamma;

  // p2 和 p2_tilde
  P.p2 = (P.p_gamma + sqrt(P.p_gamma * P.p_gamma + 4.0 * lambda * lambda * gamma / P.p1_tilde))
         / (2.0 * lambda * lambda);
  P.p2_tilde = gamma + lambda * lambda * P.p2;

  // fN 和 fM
  P.fN = (1.0 / P.p2_tilde) * (lambda * lambda * P.p2 / P.p1_tilde);
  P.fM = (1.0 / P.p1_tilde) - P.fN;

  return P;
}

// inline StateSpace tf_to_ss_proper(const TF& H_in) {
//   if (H_in.den.empty()) throw std::runtime_error("TF.den is empty.");
//   TF H = H_in;
//
//   // 归一化：den[0] 必须为 1
//   double d0 = H.den[0];
//   if (std::abs(d0) <= 1e-15) throw std::runtime_error("TF.den[0] must be nonzero.");
//   if (std::abs(d0 - 1.0) > 1e-12) {
//     for (double& a : H.den) a /= d0;
//     for (double& b : H.num) b /= d0;
//   }
//
//   const int n = (int)H.den.size() - 1;         // 阶数
//   const int m = (int)H.num.size() - 1;
//   if (n < 0) throw std::runtime_error("TF.den must have at least 1 element.");
//   if (m > n) throw std::runtime_error("tf_to_ss_proper expects a proper TF (deg(num) <= deg(den)).");
//
//   StateSpace S;
//   if (n == 0) {
//     // 纯静态：y = D u
//     S.A.resize(0,0);
//     S.B.resize(0,1);
//     S.C.resize(1,0);
//     S.D = Eigen::MatrixXd::Constant(1,1, H.num.empty()? 0.0 : H.num[0]);
//     return S;
//   }
//
//   // 填满 num 到 n+1 长度：b0..bn
//   std::vector<double> num = H.num;
//   num.resize(n+1, 0.0);
//
//   // A（z^-1 可控规范形）
//   S.A = Eigen::MatrixXd::Zero(n,n);
//   for (int i=0; i<n-1; ++i) S.A(i,i+1) = 1.0;
//   for (int j=0; j<n; ++j)   S.A(n-1,j) = -H.den[j+1];   // -a1..-an
//
//   // B
//   S.B = Eigen::VectorXd::Zero(n);
//   S.B(n-1) = 1.0;
//   S.B.resize(n,1);
//
//   // C, D
//   const double D0 = num[0];             // b0
//   S.C = Eigen::RowVectorXd(n);
//   for (int i=0; i<n; ++i) {
//     const double ai = H.den[i+1];       // a1..an
//     const double bi = num[i+1];         // b1..bn
//     S.C(i) = bi - ai * D0;
//   }
//   S.C.resize(1,n);
//   S.D = Eigen::MatrixXd::Constant(1,1, D0);
//   return S;
// }

// 由论文参数构造每个模态的 \tilde M_i(z)
// 由论文参数 + 控制器 构造闭环 \wide tilde M_i(z)
// 输入：lambda_i, tau, gamma, ktilde(TF)
// 输出：StateSpace (d=[w~,nu~] → z=[xi, chi, mu])
StateSpace Mi_from_params(const PaperParams& P, double lambda, const TF& ktilde) {
  // ---- 对象 (模态子系统) ----
  Eigen::Matrix2d Aobj;
  Aobj << 1.0, -P.fN,
           0.0,  P.fM;
  Eigen::Vector2d B2;  // control input u = mu~
  B2 << P.tau * lambda, 0.0;
  Eigen::MatrixXd B1 = Eigen::MatrixXd::Zero(2,2); // d=[w~,nu~]
  double c = lambda / std::sqrt(P.p2_tilde);
  B1(0,0) = c; B1(1,0) = c; // w~ 进入状态
  // C2, D21 (for y = theta~)
  Eigen::RowVector2d C2; C2 << std::sqrt(P.gamma), 0.0;
  Eigen::RowVector2d D21; D21 << 0.0, 1.0;

  // ---- 控制器 ktilde(z) (SISO) 转状态空间 ----
  StateSpace K = tf_to_ss_proper(ktilde); // 你已有的函数

  // ---- 拼闭环 (下LFT) ----
  int nx  = 2;        // object state
  int nxk = K.A.rows();
  int nd  = 2;        // d = [w~,nu~]
  int nz  = 3;        // z = [xi,chi,mu]

  // 大系统状态: [x; xk]
  Eigen::MatrixXd Acl = Eigen::MatrixXd::Zero(nx+nxk,nx+nxk);
  Eigen::MatrixXd Bcl = Eigen::MatrixXd::Zero(nx+nxk, nd);
  Eigen::MatrixXd Ccl = Eigen::MatrixXd::Zero(nz, nx+nxk);
  Eigen::MatrixXd Dcl = Eigen::MatrixXd::Zero(nz, nd);

  // 快捷别名
  Eigen::MatrixXd Dk = K.D; // 1x1
  Eigen::MatrixXd Ck = K.C; // 1 x nxk
  Eigen::MatrixXd Bk = K.B; // nxk x 1
  Eigen::MatrixXd Ak = K.A;

  // 对象闭环动态
  Eigen::MatrixXd Ad = Aobj + B2 * Dk * C2;       // nx x nx
  Eigen::MatrixXd Bd_xk = B2 * Ck;                // nx x nxk
  Eigen::MatrixXd Bd_d  = B1 + B2 * Dk * D21;     // nx x 2

  Eigen::MatrixXd Ak_x = Bk * C2;                 // nxk x nx
  Eigen::MatrixXd Ak_d = Bk * D21;                // nxk x 2

  // 拼 A
  Acl.block(0,0,nx,nx) = Ad;
  if (nxk>0) {
    Acl.block(0,nx,nx,nxk) = Bd_xk;
    Acl.block(nx,0,nxk,nx) = Ak_x;
    Acl.block(nx,nx,nxk,nxk) = Ak;
  }
  // 拼 B
  Bcl.block(0,0,nx,nd) = Bd_d;
  if (nxk>0) Bcl.block(nx,0,nxk,nd) = Ak_d;

  // 输出 z = [x; mu]
  // x 部分
  Ccl.block(0,0,2,nx) = Eigen::MatrixXd::Identity(2,nx);
  // mu = Ck xk + Dk(C2 x + D21 d)
  Ccl.block(2,0,1,nx) = Dk * C2;
  if (nxk>0) Ccl.block(2,nx,1,nxk) = Ck;
  Dcl.block(2,0,1,nd) = Dk * D21;

  StateSpace M;
  M.A = Acl; M.B = Bcl; M.C = Ccl; M.D = Dcl;
  return M;
}







// 依赖：synth_matlab_bridge.h（内含 matlab_synthesize_ktilde 与 MatlabKtildeReport）
#include "synth_matlab_bridge.h"

// // // // 如果你不再需要 worN，可以保留形参以兼容旧接口；内部标记未使用
// // // bool feasible_gamma(const std::vector<double>& lambdas, double tau,
// // //                     double gamma, int worN) {
// // //   (void)worN; // 未使用，仅为保持旧签名
// // //
// // //   std::cout << "gamma is " << gamma << std::endl;
// // //
// // //   // === 1) MATLAB 路径配置（与原先一致）===
// // //   const std::string matlabExe = "E:\\matlab\\bin\\matlab.exe";
// // //   const std::string mfileDir  = "D:\\timesync\\matlab";
// // //
// // //   // === 2) 调 MATLAB：一次性把全部 lambdas 传入做“全模态校验” ===
// // //   TF ktilde;                       // 仍然会返回 ktilde(z) 的系数（可忽略）
// // //   std::string err;
// // //   tsync::MatlabKtildeReport rep;   // 接收 MATLAB 的 ok 与逐模态 H∞ 范数
// // //
// // //   // lambda_max 仍用整组特征值的最大值（与你原流程一致）
// // //   const double lambda_max = *std::max_element(lambdas.begin(), lambdas.end());
// // //
// // //   bool ok = tsync::matlab_synthesize_ktilde(
// // //       matlabExe, mfileDir,
// // //       /*tau*/ tau, /*lambda_max*/ lambda_max, /*gamma*/ gamma,
// // //       /*out ktilde*/ ktilde,
// // //       /*err*/ &err,
// // //       /*lambdas_all*/ &lambdas,
// // //       /*out_report*/ &rep
// // //   );
// //
// //   if (!ok) {
// //     std::cerr << "MATLAB synthesis FAILED: " << err << "\n";
// //     return false;
// //   }
//
//   // === 3) 直接使用 MATLAB 的判定结果 ===
//   // rep.ok == true 代表：所有 |M_i|_inf < 1（近零 λ 已在 MATLAB 侧跳过）
//   // 同时输出每个模态的范数，便于调试
//   if (!rep.hinf_each_mode.empty()) {
//     for (size_t i = 0; i < rep.hinf_each_mode.size(); ++i) {
//       double lam = (i < rep.lambdas.size() ? rep.lambdas[i] : std::numeric_limits<double>::quiet_NaN());
//       double v   = rep.hinf_each_mode[i]; // 近零 λ 位置会是 NaN（由 null 转换）
//       if (std::isnan(v)) {
//         std::cout << "lambda[" << i << "]=" << lam << " skipped (near-zero)\n";
//       } else {
//         std::cout << "lambda[" << i << "]=" << lam << "  Hinf=" << v << "\n";
//       }
//     }
//   }
//
//   if (rep.ok) {
//     std::cout << "gamma " << gamma << " is feasible (all modes < 1)\n";
//   } else {
//     std::cout << "gamma " << gamma << " is NOT feasible (some mode >= 1)\n";
//   }
//   return rep.ok;
// }

// 新签名：多两个可选输出参数（其中 out_report 你已有时可复用）
bool feasible_gamma(const std::vector<double>& lambdas, double tau,
                    double gamma,
                    TF* out_ktilde,                         // 可为 nullptr
                    tsync::MatlabKtildeReport* out_report)  // 可为 nullptr
{

  std::cout << "gamma is " << gamma << std::endl;

  // 1) MATLAB 路径
  const std::string matlabExe = "E:\\matlab\\bin\\matlab.exe";
  const std::string mfileDir  = "D:\\timesync\\matlab";

  // 2) 调 MATLAB
  TF ktilde_tmp;                    // 临时承接 \tilde k(z)
  std::string err;
  tsync::MatlabKtildeReport rep;    // 本次报告

  const double lambda_max = *std::max_element(lambdas.begin(), lambdas.end());

  bool ok = tsync::matlab_synthesize_ktilde(
      matlabExe, mfileDir,
      /*tau*/ tau, /*lambda_max*/ lambda_max, /*gamma*/ gamma,
      /*out ktilde*/ ktilde_tmp,
      /*err*/ &err,
      /*lambdas_all*/ &lambdas,
      /*out_report*/ &rep
  );

  if (!ok) {
    std::cerr << "MATLAB synthesis FAILED: " << err << "\n";
    return false;
  }

  // 打印逐模态范数（与原逻辑一致）
  if (!rep.hinf_each_mode.empty()) {
    for (size_t i = 0; i < rep.hinf_each_mode.size(); ++i) {
      double lam = (i < rep.lambdas.size() ? rep.lambdas[i]
                                           : std::numeric_limits<double>::quiet_NaN());
      double v   = rep.hinf_each_mode[i];
      if (std::isnan(v)) {
        std::cout << "lambda[" << i << "]=" << lam << " skipped (near-zero)\n";
      } else {
        std::cout << "lambda[" << i << "]=" << lam << "  Hinf=" << v << "\n";
      }
    }
  }

  if (rep.ok) {
    std::cout << "gamma " << gamma << " is feasible (all modes < 1)\n";
    if (out_ktilde) *out_ktilde = std::move(ktilde_tmp); // ★ 把本次可行的 ktilde 带回
  } else {
    std::cout << "gamma " << gamma << " is NOT feasible (some mode >= 1)\n";
  }

  if (out_report) *out_report = std::move(rep);
  return rep.ok;
}

// double search_gamma(const std::vector<double>& lambdas, double tau,
//                     double g_lo, double g_hi,
//                     double tol, int max_iter) {
//   double lo = g_lo, hi = g_hi;
//   // 扩张 区间（确保 hi 可行）
//   for (int i=0;i<20 && !feasible_gamma(lambdas,tau,hi,4096); ++i) hi *= 2.0;
//   for (int it=0; it<max_iter && hi-lo>tol; ++it) {
//     double mid = 0.5*(lo+hi);
//     if (feasible_gamma(lambdas,tau,mid,4096)) hi = mid; else lo = mid;
//   }
//   return hi;
// }

double search_gamma(const std::vector<double>& lambdas,
                    double tau,
                    double g_lo,
                    double g_hi,
                    TF& ktilde_best,double tol, int max_iter){
  double lo = g_lo, hi = g_hi;

  // === 先把 hi 扩张到可行（若需要）===
  {
    TF ktmp;
    bool feasible = feasible_gamma(lambdas, tau, hi,&ktmp , nullptr);
    for (int i=0; i<20 && !feasible; ++i) {
      hi *= 2.0;
      feasible = feasible_gamma(lambdas, tau, hi,  &ktmp, nullptr);
    }
    if (!feasible) {
      // 仍不可行：返回 hi，并且不触碰 ktilde_best（或根据需要抛异常/返回 NaN）
      std::cerr << "[search_gamma] cannot find feasible upper bound after expansion.\n";
      return hi;
    }
    // 记录“目前最小可行”的 ktilde
    ktilde_best = std::move(ktmp);
  }

  // === 二分最小可行 γ* ===
  for (int it = 0; it < max_iter && hi - lo > tol; ++it) {
    double mid = 0.5 * (lo + hi);
    TF ktmp;
    if (feasible_gamma(lambdas, tau, mid, &ktmp, nullptr)) {
      // mid 可行：更新上界并保存这次的 ktilde
      hi = mid;
      ktilde_best = std::move(ktmp);   // ★ 使 ktilde_best 与“当前 hi”对应
    } else {
      lo = mid;
    }
  }
  return hi;  // ★ 返回的 hi 与 ktilde_best 同步：hi 为最终最小可行 γ*
}


void tf_to_state_space(const TF& K, std::vector<double>& A,
                       std::vector<double>& B, std::vector<double>& C, double& D) {
  StateSpace S = tf_to_ss_proper(K);
  const int n = S.nx();
  A.resize(n*n); B.resize(n); C.resize(n);
  for (int i=0;i<n;i++) for (int j=0;j<n;j++) A[i*n+j] = S.A(i,j);
  for (int i=0;i<n;i++) { B[i]=S.B(i,0); C[i]=S.C(0,i); }
  D = (S.D.size()? S.D(0,0) : 0.0);
}


std::string dump_tf(const TF& H) {
  std::ostringstream os;
  os << "num: ";
  for (size_t i=0;i<H.num.size();++i) { if(i) os<<", "; os<<H.num[i]; }
  os << "\nden: ";
  for (size_t i=0;i<H.den.size();++i) { if(i) os<<", "; os<<H.den[i]; }
  os << "\n";
  return os.str();
}






SynthResult synth_controller(const std::vector<double>& lambdas,
                             double tau,
                              TF& ktilde_proto,
                             double g_lo, double g_hi) {
  // 1) 二分最小可行 γ*
  double gamma_star = search_gamma(lambdas, tau,  g_lo, g_hi,ktilde_proto);

  // 2) 构造 k(z) = sqrt(γ*) * \tilde k(z)
  TF ktilde = ktilde_proto;
  TF k; k.num = ktilde.num; k.den = ktilde.den;
  double s = std::sqrt(gamma_star);
  for (auto& b : k.num) b *= s;

  // 3) 转状态空间
  std::vector<double> A,B,C; double D;
  tf_to_state_space(k, A,B,C,D);

  // 4) 保存到文件（使用 kz_io.h 提供的 save_kz_json）
  const std::string out_path = "kz.json";
  if (!save_kz_json(k, out_path, /*gamma=*/gamma_star, /*precision=*/17)) {
    throw std::runtime_error("save_kz_json failed: " + out_path);
  }

  return SynthResult{ ktilde, k, gamma_star, A,B,C,D };
}


